import java.util.Vector;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;

/**
 * A class that handles the Ultra sound sensor.
 *  
 * @author sava
 * 
 */
public class USSensor {
	public static UltrasonicSensor sensor = new UltrasonicSensor(SensorPort.S1);

	/**
	 * Scans in nScans directions with the same angle between them around the robot and returns the distance in each direction.
	 * @param nScans
	 * @return A vector of distances in each direction, starting at 0 degrees.
	 * @throws InterruptedException
	 */
	public static Vector<Integer> scan(int nScans) {
		Vector<Integer> scans = new Vector<Integer>(nScans);

		int degrees = 0;
		int degreesChange = 360 / nScans;

		Motor.C.setSpeed(60);
		
		for (int i = 0; i < nScans; i++) {
			//Thread.sleep(200);
			scans.addElement(new Integer(sensor.getDistance())); // Add distance
																	// to vector
			Motor.C.rotate(-degreesChange);
			degrees += degreesChange; // Accumulate degrees moved
		}

		Motor.C.setSpeed(720);
		
		Motor.C.rotate(degrees); // Go back to 0

		return scans;
	}

	/**
	 * Measures the distance forward in the robots current position.
	 * @return The distance to blocking objects forward.
	 */
	public static int scanForward() {
		return sensor.getDistance();
	}

	/**
	 * Measures the distance in degrees degrees direction. Forward is 0 degrees.
	 * @param degrees
	 * @return The distance to blocking objects in degrees angle of the robot direction.
	 */
	public static int scanDirection(int degrees) {
		Motor.C.rotate(degrees);
		int d = sensor.getDistance();
		Motor.C.rotate(-degrees);
		return d;
	}
}
